/****************************************Copyright (c)****************************************************
**                                      
**                                 http://www.powermcu.com
**
**--------------File Info---------------------------------------------------------------------------------
** File name:               main.c
** Descriptions:            The TIM application function
**
**--------------------------------------------------------------------------------------------------------
** Created by:              AVRman
** Created date:            2010-10-30
** Version:                 v1.0
** Descriptions:            The original version
**
**--------------------------------------------------------------------------------------------------------
** Modified by:             
** Modified date:           
** Version:                 
** Descriptions:            
**
*********************************************************************************************************/
//#define __GNUC__


/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include <stdio.h>
#include "Code0_PARAMETER.h"
#include "Code0_CONF.c"  
#include "Code1_MOTOR.c"
#include "Code2_COMP.c"
#include "Code3_DEBUG.c"
#include "Code7_MOTION_FUNCTION.c"
#include "Code4_MOTION.c"		
#include "Code5_DBL.c"
#include "Code6_HEAD.c"
#include "Code8_IMU.c"


#include "Code0_MAIN.c"						
/* Private function prototypes -----------------------------------------------*/


/* Private varianble -----------------------------------------------*/

uint8_t Test_RX[6] = {1, 2, 3, 4, 5, 6};
#define ZIGBEE_RX_NUM 6
uint8_t Zigbee_RX[6] = {0};
uint8_t Zigbee_RX_count = 0;
const uint16_t MainLed = GPIO_Pin_13;
GPIO_TypeDef *MainLedPort = GPIOC;



/*******************************************************************************
* Function Name  : main
* Description    : Main program
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
int main(void)
{	
//	int i;		
	Bool LedOn = TRUE;	
	GPIO_Configuration();
	TIM_Configuration();
	EXTI_Configuration();							
	USART_Configuration();	 
	NVIC_Configuration();
	ADC_Configuration();	

	Motor_Init();
	Motion_Init();
	Head_Init();	
	IMU_Init();
	Comp_Init();

	#ifdef COMP
		USART_ITConfig(COMP_PORT, USART_IT_RXNE, DISABLE);	
	#endif			
	
		
	LedSwitch(Read_IMU, 7);	
	// Change the step time to 0.1 (0.01 * 10)
	Add_motor_job(  253,  10,   0,   0,   0 );

  /* Infinite loop */
  while (1){
//		if(MotorData.Battery[LEFT_LEG] < 155)
//		{
//			LedSwitch(ON, 1);	
//		}
//		else if(MotorData.Battery[LEFT_LEG] < 160)
//		{														 
//			LedSwitch(!GetLedStatus(1), 1);	 
//		}					
//		else
//		{								
//			LedSwitch(OFF, 1);	
//		}						 
//		if(MotorData.Battery[RIGHT_LEG] < 155)
//		{																 
//			LedSwitch(ON, 4);	
//		}
//		else if(MotorData.Battery[LEFT_LEG] < 160)
//		{																
//			LedSwitch(!GetLedStatus(4), 4);
//		}									
//		else
//		{								
//			LedSwitch(OFF, 4);	
//		}						 				
//		if(MotorData.Battery[UPPER_BODY] < 155)
//		{																
//			LedSwitch(ON, 3);	
//		}
//		else if(MotorData.Battery[UPPER_BODY] < 160)
//		{													
//			LedSwitch(!GetLedStatus(3), 3);
//		}										
//		else
//		{								
//			LedSwitch(OFF, 3);	
//		}						 

//		for(i=1; i < 8; i++)
//		{
			LedSwitch(ON, 5);	
			Delay(0xFFFFF);		
//		}
//		for(i=1; i < 8; i++)
//		{
			LedSwitch(OFF, 5);	
			Delay(0xFFFFF);		
//		}	
		if(LedOn)
		{			 
			GPIO_SetBits(MainLedPort , MainLed);
		}
		else
		{
			GPIO_ResetBits(MainLedPort , MainLed);
		}
		LedOn = !LedOn;

  }
}

/*******************************************************************************
* Function Name  : EXTI9_5_IRQHandler
* Description    : This function handles External lines 0 interrupt request.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void EXTI9_5_IRQHandler(void)
{																					 
	if ( EXTI_GetITStatus(EXTI_Line5) != RESET ) 
	{									
		//static int count = 0;
		EXTI_ClearITPendingBit(EXTI_Line5);
		
//		count++;	 
//		if(count > 10)
//		{
//			LedSwitch(!GetLedStatus(6), 6);
//			count = 0;
//		}
		IMU_Run();
		RM146();
	}
	if ( EXTI_GetITStatus(EXTI_Line7) != RESET ) 
	{
		EXTI_ClearITPendingBit(EXTI_Line7);

		// BTN1 -> Initial			
		if(!GPIO_ReadInputDataBit(GPIOF, GPIO_Pin_12) && (!CallInitialize))  
		{
			//LedSwitch(!GetLedStatus(1), 1);			
			LedSwitch(!GetLedStatus(6), 6);
			DipMode = ReadDip() + 100;			
			ReceiceFromComp = FALSE;	
			Comp_Receive_Init();	
			CallInitialize = TRUE;
									 
			#ifdef COMP
				USART_ITConfig(COMP_PORT, USART_IT_RXNE, DISABLE);	
			#endif
		}									

		printf("BTN 1 Triggered\r\n");		
	}				
	else if ( EXTI_GetITStatus(EXTI_Line8) != RESET ) 
	{
		int i;
		EXTI_ClearITPendingBit(EXTI_Line8);	

		// BTN2 -> Strategy
		DipMode = ReadDip();				 
		ReceiceFromComp = TRUE;	
					
		#ifdef COMP
			USART_ITConfig(COMP_PORT, USART_IT_RXNE, ENABLE);	
		#endif
	 
		Read_IMU = TRUE;
		LedSwitch(Read_IMU, 7);
		
		printf("BTN 2 Triggered\r\n");
	}						
	else if ( EXTI_GetITStatus(EXTI_Line9) != RESET ) 
	{								 		
		EXTI_ClearITPendingBit(EXTI_Line9);		
				
		// BTN3 -> Set Compass
//		IMU_To_HMI = !IMU_To_HMI;
		LedSwitch(!GetLedStatus(3), 3);
		if(ReadDip() == 63)
		{
			Meg_Init();
			Read_IMU = TRUE;		
			LedSwitch(Read_IMU, 7);
			// Nod
			Add_motor_job(  0, 100,   2, 100,   0 );	  
   			Add_motor_job(255,  10,   0,   0,   0 );	 
			Add_motor_job(  0,   0,   2, 100,   0 );	  
		}
		else if(ReadDip() == 62)
		{							 
			Meg.GetMaxMin = FALSE;	   
			Read_IMU = FALSE;		
			LedSwitch(Read_IMU, 7);	
			// Shake head	  
			Add_motor_job(  1, 100,   2, 100,   0 );	  
   			Add_motor_job(255,  10,   0,   0,   0 );	 
			Add_motor_job(  1, 255,   1, 100,   0 );	  
		}
		else if(ReadDip() == 49)
		{
			Read_IMU = !Read_IMU;		
			LedSwitch(Read_IMU, 7);	
		}		
		else if(ReadDip() == 48)
		{
			static int arm = 0;
			//LedSwitch(!GetLedStatus(4), 4);
			if(arm == 0)
			{
				Mtn_Raise_Arm();
				arm = 1;
			}
			else if(arm == 1)
			{
				Mtn_Getdown_Arm();
				arm = 0;
			}								
		}
		
		printf("BTN 3 Triggered\r\n");
	}				
}	
/*******************************************************************************
* Function Name  : EXTI15_10_IRQHandler
* Description    : This function handles External lines 0 interrupt request.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void EXTI15_10_IRQHandler(void)
{
	if ( EXTI_GetITStatus(EXTI_Line10) != RESET ) 
	{												 
		int i = 0;
//		static int arm = 0;//Control the Arm raise up or put down
		EXTI_ClearITPendingBit(EXTI_Line10);
													
		// BTN4 -> Raise up/Put down the arms 		
//		LedSwitch(!GetLedStatus(4), 4);
//		if(arm == 0)
//		{
//			Mtn_Raise_Arm();
//			arm = 1;
//		}
//		else if(arm == 1)
//		{
//			Mtn_Getdown_Arm();
//			arm = 0;
//		}								
		LedSwitch(!GetLedStatus(4), 4);
//		ReceiceFromComp = FALSE;	
//		Comp_Receive_Init();
//		Motion.Mode_RX = STOP;				
//		Motion.Mode = STOP;	
								 
//		#ifdef COMP
//			USART_ITConfig(COMP_PORT, USART_IT_RXNE, DISABLE);	
//		#endif				 
		
		printf("BTN 4 Triggered\r\n");  											
}																			 
	else if (EXTI_GetITStatus(EXTI_Line11) != RESET) 
	{							
		int i = 0;				
		EXTI_ClearITPendingBit(EXTI_Line11); 
						 
		// BTN5 -> Stop moving & Disconnect with Comp 			
//		LedSwitch(!GetLedStatus(5), 5);
//		ReceiceFromComp = FALSE;	
//		Comp_Receive_Init();
//		Motion.Mode_RX = STOP;				
//		Motion.Mode = STOP;	
//								 
//		#ifdef COMP
//			USART_ITConfig(COMP_PORT, USART_IT_RXNE, DISABLE);	
//		#endif		

		printf("BTN 5 Triggered\r\n");														
	}						 
	else if(EXTI_GetITStatus(EXTI_Line12) != RESET) 
	{
		EXTI_ClearITPendingBit(EXTI_Line12);	
    			   	   																				 
		// BTN6 -> Read_IMU switch
//		Read_IMU = !Read_IMU;		
//		LedSwitch(Read_IMU, 7);		 

		printf("BTN 6 Triggered\r\n");
	}					
}
/*******************************************************************************
* Function Name  : TIM1_UP_IRQHandler
* Description    : This function handles TIM1 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void TIM1_UP_IRQHandler(void)
{											 			
	static unsigned int last = 0;
	unsigned int i = 0;
	if ( TIM_GetITStatus(TIM1 , TIM_IT_Update) != RESET ) 
	{
		TIM_ClearITPendingBit(TIM1 , TIM_FLAG_Update);	
														 
		RenewMotion();	 
//		i = TIM_GetCounter(MOTOR_WRITE_TIMER);
//		USART_SendData(DEBUG_PORT, i/100);
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}																							
//		USART_SendData(DEBUG_PORT, i%100);
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}						 		
//		
//		TIM_SetCounter(MOTOR_WRITE_TIMER, 0x0000); 
  }
}
/*******************************************************************************
* Function Name  : TIM2_IRQHandler
* Description    : This function handles TIM2 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void TIM2_IRQHandler(void)
{							 
	if ( TIM_GetITStatus(TIM2 , TIM_IT_Update) != RESET ) 
	{
		TIM_ClearITPendingBit(TIM2 , TIM_FLAG_Update);
		
//		TIM_SetCounter(MOTOR_WRITE_TIMER, 0x0000); 
		RenewReadID();
//		RenewReadIMU();
//		USART_SendData(DEBUG_PORT, TIM_GetCounter(MOTOR_WRITE_TIMER));
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}							
  }	
}
/*******************************************************************************
* Function Name  : TIM3_IRQHandler
* Description    : This function handles TIM3 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void TIM3_IRQHandler(void)
{
	static Bool LedOn = TRUE;	
	if (TIM_GetITStatus(TIM3 , TIM_IT_Update) != RESET) 
	{
//		TIM_ClearITPendingBit(TIM3 , TIM_FLAG_Update);
//		IMU_Run();
//		RM146();
//		if(IMU_To_HMI == TRUE)
//		{
//			IMU_SendAll(DEBUG_PORT);
//		}								 
  }	

	if(LedOn)
	{			 
		GPIO_SetBits(MainLedPort , MainLed);
	}
	else
	{
		GPIO_ResetBits(MainLedPort , MainLed);
	}
	LedOn = !LedOn;
}				 											 
/*******************************************************************************
* Function Name  : TIM4_IRQHandler
* Description    : This function handles TIM3 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void TIM4_IRQHandler(void)
{
	if ( TIM_GetITStatus(TIM4 , TIM_IT_Update) != RESET ) 
	{
		TIM_ClearITPendingBit(TIM4 , TIM_FLAG_Update);
		
  }	
}
/*******************************************************************************
* Function Name  : USART1_IRQHandler
* Description    : This function handles USART1 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
#ifdef USER_USART1_IT_RX
void USART1_IRQHandler(void)
{						
	static int count1 = 0;   	
  if(USART_GetFlagStatus(USART1, USART_FLAG_ORE) == SET)
  {
    USART_ClearFlag(USART1, USART_FLAG_ORE);	// Read SR
    USART_ReceiveData(USART1);								// Read DR
  }
	if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)   
	{
		USART_ClearITPendingBit(USART1, USART_IT_RXNE);   
	
		MotorRX[UPPER_BODY].Buff[(MotorRX[UPPER_BODY].EndIndex)] = USART_ReceiveData(USART1);
		MotorRX[UPPER_BODY].EndIndex = (MotorRX[UPPER_BODY].EndIndex + 1) % MOTOR_RX_BUFF_SIZE;
						
		count1++;																					
		if(count1 >= 13)	
		{
			count1 = 0;			 						
			CheckReceive(UPPER_BODY);
		}
	}
	if (USART_GetITStatus(USART1, USART_IT_TXE) != RESET) 
	{
		USART_ClearITPendingBit(USART1, USART_IT_TXE);
	} 
	if (USART_GetITStatus(USART1, USART_IT_TC) != RESET) 
	{
		USART_ClearITPendingBit(USART1, USART_IT_TC);
	} 
}			
#endif
/*******************************************************************************
* Function Name  : USART2_IRQHandler
* Description    : This function handles USART1 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/ 
#ifdef USER_USART2_IT_RX
void USART2_IRQHandler(void)
{										
	static int count2 = 0;   			
  if(USART_GetFlagStatus(USART2, USART_FLAG_ORE) == SET)
  {
    USART_ClearFlag(USART2, USART_FLAG_ORE);	// Read SR
    USART_ReceiveData(USART2);								// Read DR
  }                    
	if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)   
	{
		USART_ClearITPendingBit(USART2, USART_IT_RXNE); 
	
		MotorRX[LEFT_LEG].Buff[(MotorRX[LEFT_LEG].EndIndex)] = USART_ReceiveData(USART2);
		MotorRX[LEFT_LEG].EndIndex = (MotorRX[LEFT_LEG].EndIndex + 1) % MOTOR_RX_BUFF_SIZE;
						
		count2++;																					
		if(count2 >= 13)	
		{
			count2 = 0;			 							
			CheckReceive(LEFT_LEG);
		}
	}
	if (USART_GetITStatus(USART2, USART_IT_TXE) != RESET) 
	{
		USART_ClearITPendingBit(USART2, USART_IT_TXE);
	} 
	if (USART_GetITStatus(USART2, USART_IT_TC) != RESET) 
	{
		USART_ClearITPendingBit(USART2, USART_IT_TC);
	} 
}			
#endif
/*******************************************************************************
* Function Name  : USART3_IRQHandler
* Description    : This function handles USART1 Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/		
#ifdef USER_USART3_IT_RX
void USART3_IRQHandler(void)
{										    
	static int count3 = 0;   					 
  if(USART_GetFlagStatus(USART3, USART_FLAG_ORE) == SET)
  {
    USART_ClearFlag(USART3, USART_FLAG_ORE);	// Read SR
    USART_ReceiveData(USART3);								// Read DR
  }
	if (USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)   
	{
		USART_ClearITPendingBit(USART3, USART_IT_RXNE); 
	
		MotorRX[RIGHT_LEG].Buff[(MotorRX[RIGHT_LEG].EndIndex)] = USART_ReceiveData(USART3);	 
		MotorRX[RIGHT_LEG].EndIndex = (MotorRX[RIGHT_LEG].EndIndex + 1) % MOTOR_RX_BUFF_SIZE;
				
		count3++;																					
		if(count3 >= 13)	
		{
			count3 = 0;			 
			CheckReceive(RIGHT_LEG);
		}
						 											
	}
	if (USART_GetITStatus(USART3, USART_IT_TXE) != RESET) 
	{
		USART_ClearITPendingBit(USART3, USART_IT_TXE);
	} 
	if (USART_GetITStatus(USART3, USART_IT_TC) != RESET) 
	{
		USART_ClearITPendingBit(USART3, USART_IT_TC);
	} 
}								 
#endif
/*******************************************************************************
* Function Name  : UART4_IRQHandler
* Description    : This function handles USART4 (DEBUG_PORT) Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void UART4_IRQHandler(void)
{						
	static int count4 = 0;		 
  if(USART_GetFlagStatus(UART4, USART_FLAG_ORE) == SET)
  {
    USART_ClearFlag(UART4, USART_FLAG_ORE);	// Read SR
    USART_ReceiveData(UART4);								// Read DR
  }
	if (USART_GetITStatus(UART4, USART_IT_RXNE) != RESET)   
	{
		USART_ClearITPendingBit(UART4, USART_IT_RXNE);
		
		ZigbeeRX[count4] = USART_ReceiveData(DEBUG_PORT);	
		count4++;

		if(count4 >= ZIGBEE_RX_NUM)
		{
			if(Zigbee_SendMotorData == FALSE)
			{
				if(ZigbeeAccessRX() == CORRECT)
				{
					LedSwitch(!GetLedStatus(5), 5);
				}
			}
			count4 = 0;
		}
	} 		 
	if(USART_GetITStatus(UART4, USART_IT_TC) != RESET)
	{
		USART_ClearFlag(UART4,USART_FLAG_TC);
	}
}		
/*******************************************************************************
* Function Name  : UART5_IRQHandler
* Description    : This function handles UART5 (COMP_PORT) Handler.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void UART5_IRQHandler(void)
{
	static int count3 = 0;
	unsigned char read_byte = 0;		
  if(USART_GetFlagStatus(UART5, USART_FLAG_ORE) == SET)
  {
    USART_ClearFlag(UART5, USART_FLAG_ORE);	// Read SR
    USART_ReceiveData(UART5);								// Read DR
  }
	if (USART_GetITStatus(UART5, USART_IT_RXNE) != RESET)   
	{
		USART_ClearITPendingBit(UART5, USART_IT_RXNE);									
		
		#ifdef COMP																				
		read_byte = USART_ReceiveData(COMP_PORT);
		#endif
		
//		USART_SendData(DEBUG_PORT, read_byte);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}				
														
		CompRX.Buff[CompRX.EndIndex] = read_byte;			
		CompRX.EndIndex = (CompRX.EndIndex + 1) % COMP_RX_BUFF_SIZE;	 
		count3++;
//
//		#ifdef IMU_TO_SLAVE				
			if(count3 >= COMP_R_LENGTH)
			{									
//				if(CompRX2Receive() == TRUE)
//				{																	 
//					LedSwitch(!GetLedStatus(6), 6);
//					SlaveSend();
//				}				
				count3 = 0;
			}		  													
//		#else
//			#ifdef SLAVE							 
//				USART_SendData(SLAVE_PORT, read_byte); 		 		
//				while(USART_GetFlagStatus(SLAVE_PORT, USART_FLAG_TXE) == RESET)
//				{
//				}
//			#endif 		
			
//			if(count3 >= COMP_R_LENGTH)
//			{													
//				LedSwitch(!GetLedStatus(6), 6);
//				count3 = 0;													 
//			}		  							 
//		#endif 	
						
	} 		
	if(USART_GetITStatus(UART5, USART_IT_TC) != RESET)
	{
		USART_ClearFlag(UART5,USART_FLAG_TC);
	}
}



void DMA1_Channel4_IRQHandler(void)
{
	int i;
	if(DMA_GetITStatus(DMA1_IT_TC4) == SET)
  {
	  DMA_ClearITPendingBit(DMA1_IT_GL4); 
		MotorWritedFinish[UPPER_BODY] = TRUE;
																						
		for(i=0; i < MOTOR_NUM_UB; i++)
		{
			MotorData.Write_UB[i][HIGH_POSITION] |= 0x10;
		}			
	}				 
	else
	{	 
	  DMA_ClearITPendingBit(DMA1_IT_GL4); 
	}
}														 

void DMA1_Channel7_IRQHandler(void)
{								
	int i;
	if(DMA_GetITStatus(DMA1_IT_TC7) == SET)
  {
	  DMA_ClearITPendingBit(DMA1_IT_GL7); 
		MotorWritedFinish[LEFT_LEG] = TRUE;	 
																						
		for(i=0; i < MOTOR_NUM_LL; i++)
		{
			MotorData.Write_LL[i][HIGH_POSITION] |= 0x10;
		}
	}	 
	else
	{	 
	  DMA_ClearITPendingBit(DMA1_IT_GL7); 
	}
}

void DMA1_Channel2_IRQHandler(void)
{								
	int i;
	if(DMA_GetITStatus(DMA1_IT_TC2) == SET)
  {
	  DMA_ClearITPendingBit(DMA1_IT_GL2); 
		MotorWritedFinish[RIGHT_LEG] = TRUE; 
																						
		for(i=0; i < MOTOR_NUM_RL; i++)
		{
			MotorData.Write_RL[i][HIGH_POSITION] |= 0x10;
		}
	}
	else
	{	 
	  DMA_ClearITPendingBit(DMA1_IT_GL2); 
	}
}

void DMA2_Channel4_5_IRQHandler(void)
{								
	if(DMA_GetITStatus(DMA2_IT_TC5) == SET)
  {
	  DMA_ClearITPendingBit(DMA2_IT_GL5); 

		Zigbee_SendMotorData = FALSE;
	}
	else
	{	 
	  DMA_ClearITPendingBit(DMA2_IT_GL5); 
	}
}

#ifdef  USE_FULL_ASSERT

/**
  * @brief  Reports the name of the source file and the source line number
  *   where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_faiLed(uint8_t* file, uint32_t line)
{ 
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

  /* Infinite loop */
  while (1)
  {
  }
}
#endif
/*********************************************************************************************************
      END FILE
*********************************************************************************************************/

